%author: wxj233
%time: 2024.1.11 12:00
%function: 主要用于完成论文画图
mfpTracer = load("mfpTracer.mat", "mfpTracer").mfpTracer;
clusterTracer = load("clusterTracer.mat", "clusterTracer").clusterTracer;

figure(1)  % 跟踪轨迹图
rT3 = load("rt3.mat", "rT3").rT3;
plot(rT3(:, 2), rT3(:, 3), 'DisplayName', "目标真实轨迹");hold on;

fp = clusterTracer.deadFps(1);
plot(fp.points(:,13), fp.points(:,15),'DisplayName', "聚类中心方法");hold on;

for track = mfpTracer.deadTracks  % [1t, 2x, 3vx, 4y,5vy; ...]
    if ~isempty(track.points) && track.id == 5
        te = "Track"+num2str(track.id);
        plot(track.points(:,2), track.points(:,4),'DisplayName', "ET-SC");hold on;
    end
end

title("目标跟踪轨迹");
xlabel("x(m)");
ylabel("y(m)");
legend;

figure(2)  % 跟踪差误图
rT3 = load("rt3.mat", "rT3").rT3;

fp = clusterTracer.deadFps(1);
points = fp.points(:, [1, 13,15]);
points(:,1) = round(points(:,1), 3);

Ic = ismember(rT3(:,1), points(:, 1));
x = fp.points(:,13)-rT3(Ic, 2);
y = fp.points(:,15)-rT3(Ic, 3);
plot(fp.points(:,1), sqrt(x.*x+y.*y), 'DisplayName', "聚类中心方法");hold on;


for track = mfpTracer.deadTracks  % [1t, 2x, 3vx, 4y,5vy; ...]
    if ~isempty(track.points) && track.id == 5
        Ic = ismember(rT3(:,1), track.points(:,1));
        x = track.points(:,2)-rT3(Ic, 2);
        y = track.points(:,4)-rT3(Ic, 3);
        plot(track.points(:,1), sqrt(x.*x+y.*y),'DisplayName', "ET-SC");hold on;
    end
end

title("跟踪误差");
xlabel("t(s)");
ylabel("误差(m)");
legend;

